import java.io.BufferedReader;
import java.io.File;
import java.io.FileInputStream;
import java.io.FileNotFoundException;
import java.io.FileReader;
import java.io.FileWriter;
import java.io.IOException;
import java.util.ArrayList;

import org.apache.commons.math3.geometry.euclidean.threed.Vector3D;

import net.sf.json.JSONArray;
import net.sf.json.JSONObject;
import net.sf.json.JSONSerializer;

import log.Log;

public class Path {

	private static final String LOG_TAG = Path.class.getSimpleName();

	private ArrayList<Pose> path;
	//	private int current;
	//	private int next;

	public Path(ArrayList<Pose> path){
		this.path = path;
		//		current = -1;
		//		next = 0;
	}

	public Path(String filePath){
		path = jsonStringToArrayListPose(filePath);
	}

	public ArrayList<Pose> getPath() {
		return path;
	}

	public void setPath(ArrayList<Pose> path) {
		this.path = path;
	}

	private ArrayList<Pose> jsonStringToArrayListPose(String filePath){
		String line;
		StringBuffer sb = new StringBuffer();
		File jsonFile = new File(filePath);
		try {
			BufferedReader buf = new BufferedReader(new FileReader(jsonFile));
			while((line = buf.readLine()) != null){
				sb.append(line);
			}
			//Log.i(LOG_TAG, sb.toString());
			JSONArray array = (JSONArray) JSONSerializer.toJSON(sb.toString());
			ArrayList<Pose> path = new ArrayList<Pose>();
			for(int i = 0; i < array.size(); i++){
				Pose pos = new Pose();
				pos.setPoseFromJSONObject(array.getJSONObject(i));  
				path.add(pos);
			}
			return path;
			//Log.i(LOG_TAG, path.toString());
		} catch (FileNotFoundException e) {
			e.printStackTrace();
		} catch (IOException e) {
			e.printStackTrace();
		}
		return null;
	}

	public void toDatFile(String datFileName, Robot r){
		File f = new File(datFileName + "_normal.dat");
		File fr = new File(datFileName + "_robot.dat");
		try {
			FileWriter fw = new FileWriter(f);
			FileWriter fwr = new FileWriter(fr);
			for(Pose p : path){
				fw.append(p.getPosition().getX() + " " + p.getPosition().getY()+"\n");
				fwr.append(p.convertPositionToRobotCoordinateSystem(r).getX() + " " + p.convertPositionToRobotCoordinateSystem(r).getY()+"\n");
			}
			fw.close();
			fwr.close();
		} catch (IOException e) {
			e.printStackTrace();
		}
	}

	public int getNextGoalPoint(double lookaheadDistance, Robot r){
		int j = 0;
		double distance = 0;
		double distanceMax = 0;
		int positionGoal = -1;
		Vector3D toCompare;
		do{
			toCompare = path.get(j).convertPositionToRobotCoordinateSystem(r);
			distance = toCompare.getNorm();
			if(distance > distanceMax && distance < lookaheadDistance){
				distanceMax = distance;
				positionGoal = j;
			}
			j++;
		}while(j < path.size() && distanceMax < lookaheadDistance);
		if(positionGoal == -1){
			positionGoal = 0;
			double tmp;
			toCompare = path.get(0).convertPositionToRobotCoordinateSystem(r);
			double minimalNorm = toCompare.getNorm(); 
			for(int i = 1; i < path.size(); i++){
				toCompare = path.get(i).convertPositionToRobotCoordinateSystem(r);
				tmp = toCompare.getNorm();
				if(tmp < minimalNorm){
					positionGoal = i;
					minimalNorm = tmp;
				}
			}
		}
		return positionGoal;
	}
}
